#include "driver/gpio.h"
#include "driver/spi_master.h"
#include "esp_err.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"


#define PIN_MOTOR_AP GPIO_NUM_9
#define PIN_MOTOR_AM GPIO_NUM_10
#define PIN_MOTOR_BP GPIO_NUM_11
#define PIN_MOTOR_BM GPIO_NUM_12

#define PIN_KEY GPIO_NUM_0

uint8_t current_motor_pos = 0;

uint8_t MOTOR_TABLE[8][4] = {
    {1, 0, 0, 1},
    {0, 0, 0, 1},
    {0, 0, 1, 1},
    {0, 0, 1, 0},
    {0, 1, 1, 0},
    {0, 1, 0, 0},
    {1, 1, 0, 0},
    {1, 0, 0, 0},
};

void bsp_step_init(){
    gpio_config_t bk_gpio_config = {
        .intr_type = GPIO_INTR_DISABLE,
        .mode = GPIO_MODE_OUTPUT,
        .pin_bit_mask = ((1ULL << PIN_MOTOR_AP) | (1ULL << PIN_MOTOR_AM) | (1ULL << PIN_MOTOR_BP) | (1ULL << PIN_MOTOR_BM))
    };
    ESP_ERROR_CHECK(gpio_config(&bk_gpio_config));

    gpio_set_level(PIN_MOTOR_AP,0);
    gpio_set_level(PIN_MOTOR_AM,0);
    gpio_set_level(PIN_MOTOR_BP,0);
    gpio_set_level(PIN_MOTOR_BM,0);

    // 初始化测试按键
    gpio_config_t key_gpio_config = {
        .intr_type = GPIO_INTR_DISABLE,
        .mode = GPIO_MODE_INPUT,
        .pin_bit_mask = (1ULL << PIN_KEY) 
    };
    ESP_ERROR_CHECK(gpio_config(&key_gpio_config));
}

void bsp_step_stop(){
    gpio_set_level(PIN_MOTOR_AP,0);
    gpio_set_level(PIN_MOTOR_AM,0);
    gpio_set_level(PIN_MOTOR_BP,0);
    gpio_set_level(PIN_MOTOR_BM,0);
}

void bsp_step_move_one_step(){
    static TickType_t last_wake_time ;
    
    last_wake_time = xTaskGetTickCount();

    gpio_set_level(PIN_MOTOR_AP,MOTOR_TABLE[current_motor_pos][0]);
    gpio_set_level(PIN_MOTOR_BP,MOTOR_TABLE[current_motor_pos][1]);
    gpio_set_level(PIN_MOTOR_AM,MOTOR_TABLE[current_motor_pos][2]);
    gpio_set_level(PIN_MOTOR_BM,MOTOR_TABLE[current_motor_pos][3]);
    current_motor_pos = (current_motor_pos+1)%8;

   
    //vTaskDelayUntil(&last_wake_time, pdMS_TO_TICKS(2)); 
    vTaskDelay(pdMS_TO_TICKS(2));
}

void bsp_step_move_steps(uint16_t steps){
    for (size_t i = 0; i < steps; i++)
    {
        bsp_step_move_one_step();
        //vTaskDelay(pdMS_TO_TICKS(2));
        //esp_rom_delay_us(2000); 
    }
    
}

void example_step_app_main(){
    bsp_step_init();
    
    while (1)
    {
        uint8_t flag = gpio_get_level(PIN_KEY);
        if(flag == 0){
            vTaskDelay(pdMS_TO_TICKS(80));
            flag = gpio_get_level(PIN_KEY);
            if(flag == 0){
                bsp_step_move_steps(100);
                bsp_step_stop();
                printf("按键走纸\r\n");
            }
        }
        vTaskDelay(10);
    }

}